/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * File Name          : freertos.c
  * Description        : Code for freertos applications
  ******************************************************************************
  * @attention
  *
  * Copyright (c) 2024 STMicroelectronics.
  * All rights reserved.
  *
  * This software is licensed under terms that can be found in the LICENSE file
  * in the root directory of this software component.
  * If no LICENSE file comes with this software, it is provided AS-IS.
  *
  ******************************************************************************
  */
/* USER CODE END Header */

/* Includes ------------------------------------------------------------------*/
#include "FreeRTOS.h"
#include "task.h"
#include "main.h"
#include "cmsis_os.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "RobotPara.h"
#include "motor.h"
#include "oled.h"
#include "PS2.h"
#include "UART.h"
#include "usart.h"
#include "stdio.h"
#include "mpu6050.h"
#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h"
#include "string.h"

/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */

/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN Variables */

/* USER CODE END Variables */
osThreadId defaultTaskHandle;
osThreadId OLED_TaskHandle;
osThreadId Motor_TaskHandle;
osThreadId PS2_TaskHandle;
osThreadId Uart1_TaskHandle;
osThreadId Uart1Cmd_TaskHandle;
osThreadId MUP6050_TaskHandle;
osThreadId Uart2_TaskHandle;
osThreadId Uart2Cmd_TaskHandle;
osMessageQId Uart1CmdHandle;
osMessageQId Uart2CmdHandle;

/* Private function prototypes -----------------------------------------------*/
/* USER CODE BEGIN FunctionPrototypes */

// 串口重定
int fputc(int ch, FILE *f)
{
	//HAL_UART_Transmit(&huart1, (uint8_t *)&ch, 1, HAL_MAX_DELAY);  // 串口1发送
	HAL_UART_Transmit(&huart2, (uint8_t *)&ch, 1, HAL_MAX_DELAY);  // 串口2发送
	return ch;
}


/* USER CODE END FunctionPrototypes */

void StartDefaultTask(void const * argument);
void StartOLED_Task(void const * argument);
void StartMotorTask(void const * argument);
void StartPS2_Task(void const * argument);
void StartUart1_Task(void const * argument);
void Start_Uart1Cmd_Task(void const * argument);
void StartMPU6050(void const * argument);
void Start_Uart2Cmd_Task(void const * argument);

void MX_FREERTOS_Init(void); /* (MISRA C 2004 rule 8.1) */

/**
  * @brief  FreeRTOS initialization
  * @param  None
  * @retval None
  */
void MX_FREERTOS_Init(void) {
  /* USER CODE BEGIN Init */

  /* USER CODE END Init */

  /* USER CODE BEGIN RTOS_MUTEX */
  /* add mutexes, ... */
  /* USER CODE END RTOS_MUTEX */

  /* USER CODE BEGIN RTOS_SEMAPHORES */
  /* add semaphores, ... */
  /* USER CODE END RTOS_SEMAPHORES */

  /* USER CODE BEGIN RTOS_TIMERS */
  /* start timers, add new ones, ... */
  /* USER CODE END RTOS_TIMERS */

  /* Create the queue(s) */
  /* definition and creation of Uart1Cmd */
  osMessageQDef(Uart1Cmd, 16, uint16_t);
  Uart1CmdHandle = osMessageCreate(osMessageQ(Uart1Cmd), NULL);

  /* definition and creation of Uart2Cmd */
  osMessageQDef(Uart2Cmd, 16, uint16_t);
  Uart2CmdHandle = osMessageCreate(osMessageQ(Uart2Cmd), NULL);

  /* USER CODE BEGIN RTOS_QUEUES */
  /* add queues, ... */
  /* USER CODE END RTOS_QUEUES */

  /* Create the thread(s) */
  /* definition and creation of defaultTask */
  osThreadDef(defaultTask, StartDefaultTask, osPriorityNormal, 0, 128);
  defaultTaskHandle = osThreadCreate(osThread(defaultTask), NULL);

  /* definition and creation of OLED_Task */
  osThreadDef(OLED_Task, StartOLED_Task, osPriorityNormal, 0, 256);
  OLED_TaskHandle = osThreadCreate(osThread(OLED_Task), NULL);

  /* definition and creation of Motor_Task */
  osThreadDef(Motor_Task, StartMotorTask, osPriorityNormal, 0, 256);
  Motor_TaskHandle = osThreadCreate(osThread(Motor_Task), NULL);

  /* definition and creation of PS2_Task */
  osThreadDef(PS2_Task, StartPS2_Task, osPriorityNormal, 0, 256);
  PS2_TaskHandle = osThreadCreate(osThread(PS2_Task), NULL);

  /* definition and creation of Uart1_Task */
  osThreadDef(Uart1_Task, StartUart1_Task, osPriorityNormal, 0, 256);
  Uart1_TaskHandle = osThreadCreate(osThread(Uart1_Task), NULL);

  /* definition and creation of Uart1Cmd_Task */
  osThreadDef(Uart1Cmd_Task, Start_Uart1Cmd_Task, osPriorityNormal, 0, 128);
  Uart1Cmd_TaskHandle = osThreadCreate(osThread(Uart1Cmd_Task), NULL);

  /* definition and creation of MUP6050_Task */
  osThreadDef(MUP6050_Task, StartMPU6050, osPriorityNormal, 0, 256);
  MUP6050_TaskHandle = osThreadCreate(osThread(MUP6050_Task), NULL);

  /* definition and creation of Uart2_Task */
  osThreadDef(Uart2_Task, StartUart1_Task, osPriorityNormal, 0, 256);
  Uart2_TaskHandle = osThreadCreate(osThread(Uart2_Task), NULL);

  /* definition and creation of Uart2Cmd_Task */
  osThreadDef(Uart2Cmd_Task, Start_Uart2Cmd_Task, osPriorityNormal, 0, 128);
  Uart2Cmd_TaskHandle = osThreadCreate(osThread(Uart2Cmd_Task), NULL);

  /* USER CODE BEGIN RTOS_THREADS */
  /* add threads, ... */
  /* USER CODE END RTOS_THREADS */

}

/* USER CODE BEGIN Header_StartDefaultTask */
/**
  * @brief  Function implementing the defaultTask thread.
  * @param  argument: Not used
  * @retval None
  */
/* USER CODE END Header_StartDefaultTask */
void StartDefaultTask(void const * argument)
{
  /* USER CODE BEGIN StartDefaultTask */
  /* Infinite loop */
  for(;;)
  {
    osDelay(10);
  }
  /* USER CODE END StartDefaultTask */
}

/* USER CODE BEGIN Header_StartOLED_Task */
/**
* @brief Function implementing the OLED_Task thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_StartOLED_Task */
void StartOLED_Task(void const * argument)
{
  /* USER CODE BEGIN StartOLED_Task */
  /* Infinite loop */
  for(;;)
  {
	OLED_Show_RobotSpeedAndPos();
    osDelay(10);
	OLED_Refresh_Gram();
  }
  /* USER CODE END StartOLED_Task */
}

/* USER CODE BEGIN Header_StartMotorTask */
/**
* @brief Function implementing the Motor_Task thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_StartMotorTask */
void StartMotorTask(void const * argument)
{
  /* USER CODE BEGIN StartMotorTask */
  /* Infinite loop */
  for(;;)
  {
	Motor_Run_Task();
    osDelay(10);
  }
  /* USER CODE END StartMotorTask */
}

/* USER CODE BEGIN Header_StartPS2_Task */
/**
* @brief Function implementing the PS2_Task thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_StartPS2_Task */
void StartPS2_Task(void const * argument)
{
  /* USER CODE BEGIN StartPS2_Task */
  /* Infinite loop */
  for(;;)
  {
	  PS2_Read();
    osDelay(10);
  }
  /* USER CODE END StartPS2_Task */
}

/* USER CODE BEGIN Header_StartUart1_Task */
/**
* @brief Function implementing the Uart1_Task thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_StartUart1_Task */
void StartUart1_Task(void const * argument)
{
  /* USER CODE BEGIN StartUart1_Task */
  /* Infinite loop */
  for(;;)
  {
//	char x_str[7], y_str[7], z_str[7];
//	sprintf(x_str, " %0.3f" , vx_meas);
//	sprintf(y_str, " %0.3f" , vy_meas);
//	sprintf(z_str, " %0.3f" , vz_meas);
//	uint8_t ss = '\n';
//	HAL_StatusTypeDef Status;
//	Status = HAL_UART_Transmit(&huart1, (uint8_t*)x_str, sizeof(x_str) - 1, HAL_MAX_DELAY);
//	Status = HAL_UART_Transmit(&huart1, &ss, 1, HAL_MAX_DELAY);
//	if(Status == HAL_OK)
//	{
//		//HAL_GPIO_TogglePin(GPIOA, GPIO_PIN_12);
//	}
//	uint8_t Enter = '\n';
//	uint8_t Blankspace = ' ';
//	uint8_t Line = '/';
	Transmit_Buf_Struct_Get(Transmit_Buf); // 得到要发送的距离与速度字符串
	// ����
//	for(int i=0; i<6; i++)
//	{
//		HAL_UART_Transmit(&huart1, (uint8_t*)Transmit_Buf[2*i].para_string, sizeof(Transmit_Buf[i].para_string) - 1, HAL_MAX_DELAY);
//		HAL_UART_Transmit(&huart1, &Line, 1, HAL_MAX_DELAY);
//		HAL_UART_Transmit(&huart1, (uint8_t*)Transmit_Buf[2*i+1].para_string, sizeof(Transmit_Buf[i].para_string) - 1, HAL_MAX_DELAY);
//		HAL_UART_Transmit(&huart1, &Enter, 1, HAL_MAX_DELAY);
//	}
//	HAL_UART_Transmit(&huart1, &Blankspace, 1, HAL_MAX_DELAY);

	printf("xs:%0.2f ys:%0.2f yz:%0.2f ds:%0.2f dy:%0.2f dz:%0.2f %0.0f %0.0f %0.0f %0.0f %0.0f %0.0f \n",\
	Transmit_Buf[0].para_string,Transmit_Buf[2].para_string,Transmit_Buf[4].para_string,\
	Transmit_Buf[6].para_string,Transmit_Buf[8].para_string,Transmit_Buf[10].para_string,\
	kp_d, ki_d, kd_d, kp_v, ki_v, kd_v);
    osDelay(10);
  }
  /* USER CODE END StartUart1_Task */
}

/* USER CODE BEGIN Header_Start_Uart1Cmd_Task */
/**
* @brief Function implementing the Uart1Cmd_Task thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_Start_Uart1Cmd_Task */
void Start_Uart1Cmd_Task(void const * argument)
{
  /* USER CODE BEGIN Start_Uart1Cmd_Task */
	// ��������UART1�Ĵ��ڷ��͵���Ϣ����PID��������������
	osEvent flag;
  /* Infinite loop */
  for(;;)
  {
	flag = osMessageGet(Uart1CmdHandle, osWaitForever); // һֱ�ȴ�����д�룬һ��д���������������ϻ���
    if(flag.status == osEventMessage) // ��ISR����Ҫ��osEventMessage
	{
		// ��ʼ������޸�?
		if(Receive_Buf.Receive_Cmd == 0x00) // �޸�pid����
		{
			kp_d = Receive_Buf.Receive_Para[0];
			ki_d = Receive_Buf.Receive_Para[1];
			kd_d = Receive_Buf.Receive_Para[2];
			kp_v = Receive_Buf.Receive_Para[3];
			ki_v = Receive_Buf.Receive_Para[4];
			kd_v = Receive_Buf.Receive_Para[5];
			// ͣ��
			Motor_Clear();
			Uart_Motor_Cmd = 0;
		}
		if(Receive_Buf.Receive_Cmd == 0x02) // ��������motor
		{
			motor_init_flag = 1;
			Uart_Motor_Cmd = 1;
		}
		Receive_Buf.Receive_Cmd = 0x01; // �������κ������״̬
	}
	osDelay(10);
	  
  }
  /* USER CODE END Start_Uart1Cmd_Task */
}

/* USER CODE BEGIN Header_StartMPU6050 */
/**
* @brief Function implementing the MUP6050_Task thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_StartMPU6050 */
void StartMPU6050(void const * argument)
{
  /* USER CODE BEGIN StartMPU6050 */
//	float pitch,roll,yaw;  // pitch：俯仰角（绕横轴旋转的角度） roll：翻滚角（绕纵轴旋转的角度）yall：偏航角（绕垂直轴旋转）
  /* Infinite loop */
  for(;;)
  {
	if(mpu_dmp_get_data(&pitch, &roll, &yaw)==0)
	{
		printf("Pitch:%f \r roll:%f\r yaw:%f\r\n",pitch,roll,yaw);
	}
    osDelay(10);
  }
  /* USER CODE END StartMPU6050 */
}

/* USER CODE BEGIN Header_Start_Uart2Cmd_Task */
/**
* @brief Function implementing the Uart2Cmd_Task thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_Start_Uart2Cmd_Task */
void Start_Uart2Cmd_Task(void const * argument)
{
  /* USER CODE BEGIN Start_Uart2Cmd_Task */
  /* Infinite loop */
  for(;;)
  {
    osDelay(1);
  }
  /* USER CODE END Start_Uart2Cmd_Task */
}

/* Private application code --------------------------------------------------*/
/* USER CODE BEGIN Application */
// ��д���ճɹ����жϷ�����
void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
{
	static uint8_t IT_CNT = 0;
	if(huart == &huart2)
	{
		RxBuffer_List[IT_CNT++] = RxBuffer;
		if(RxBuffer == 'e')
		{
			RxBuffer_List[IT_CNT++] = '\0';
			sscanf((char*)RxBuffer_List,"%hhu %f %f %f %f %f %f ", &Receive_Buf.Receive_Cmd, \
			&Receive_Buf.Receive_Para[0], &Receive_Buf.Receive_Para[1], &Receive_Buf.Receive_Para[2], \
			&Receive_Buf.Receive_Para[3], &Receive_Buf.Receive_Para[4], &Receive_Buf.Receive_Para[5]);
			IT_CNT = 0;
			osStatus status = osMessagePut(Uart1CmdHandle, (uint32_t)&Receive_Buf, 0); // ���Ѷ��ö��е���������������
			if(status != osOK)
			{
				printf("Error\n");
			}
		}
		// ��������д����е����?
		HAL_UART_Receive_IT(&huart2, &RxBuffer, 1); // ���¿��������ж�
	}
	// 蓝牙通信
//	else if(huart == &huart2)
//	{
//		
//		HAL_UART_Receive_IT(&huart2, &RxBuffer, 1);	
//	}
}

/* USER CODE END Application */
